#include "User.h"
#include "usart.h"
#include <stdint.h>

/********** 编码器相关函数 **********/
 //encode_num_init                      1-磁编码器数量初始化
// encode_update                        2-更新总路程，速度
/****************************************/

/********** 全局变量 **********/
encoder_t encoders[] = {
    {0, 0, 0, 0},
};

static uint8_t encode_num = 0;
float zero_electric_angle = 0.0f;

/*****************************/

/********** 1-外设数量初始化函数 **********/
void encode_num_init(void) {
  encode_num = sizeof(encoders) / sizeof(encoder_t);
}
/*****************************************/

/********** 2-外设底层函数函数 **********/
// 电角度求解
float _electricalAngle(float shaft_angle, int pole_pairs) {
  return (-1 * shaft_angle * pole_pairs);
}

// 归一化角度到 [0,2PI]
float _normalizeAngle(float angle) {
  float a = fmodf(angle, 2 * PI);
  a =  (a >= 0 ) ? a : (a + 2 * PI);
  return a;
}

// 获取机械角度【0-6.28】
float _get_angle(void) {
  uint16_t raw_as5600_date;
  // 读取角度（0-4095）
  HAL_StatusTypeDef angle_status = AS5600_GetRawAngle(&hi2c1, &raw_as5600_date);
  // 成功读到
  if (angle_status == HAL_OK) {
    // 映射到为弧度(0-6.28)
    float angle = AS5600_RawAngleToRadians(raw_as5600_date);
    // 正常的控制逻辑
    return angle;
  } else {
    return 1000.0f;
  }
}

uint16_t _get_uint_angle(void) {
  uint16_t raw_as5600_date = 0xf000;
  HAL_StatusTypeDef angle_status;

  angle_status = AS5600_GetScaledAngle(&hi2c1, &raw_as5600_date);
  // 成功读到
  if (angle_status == HAL_OK) {
    return raw_as5600_date;
  }
  else {
    return 0xf000;
  }
}
// 机械角度转换为电角度
float normal_to_elect(float angle) {
  float electrical_angle = _electricalAngle(angle, POLE_PAIR);// //- zero_electric_angle;
      return _normalizeAngle(electrical_angle);
}
/*************************************/

/********** 3-外设硬件调用函数 **********/
// 更新圈数，总路程和速度
void encode_update(void) {
  static uint8_t begin_flag = 1;

  if (begin_flag) {
    encoders[0].angle_now = _get_uint_angle();
    encoders[0].angle_total_now = encoders[0].angle_now;
    encoders[0].circle_num = 0;
    encoders[0].speed = 0.0f;
    begin_flag = 0;
  } else {

    if (encoders[0].angle_now != 0xf000) {
      float a = 0.02f; // 当前角度的低通滤波系数
      float b = 0.616f; // 总路程的低通滤波系数
      float c = 0.563f; // 速度的低通滤波系数

      /* 对读取的角度进行限幅滤波 */
      int angle_raw = _get_uint_angle();
      int delte_angle_raw = (abs(angle_raw - encoders[0].angle_now));
      if (((delte_angle_raw >= (900))) && (delte_angle_raw <= (3600))) {
        angle_raw = encoders[0].angle_now;
      }
      
      /* 低通滤波获取当前角度 */
      angle_raw = (int)((a * encoders[0].angle_now) + ((1 - a) * angle_raw));

      /* 多圈计数 */
      int delta_angle = angle_raw - encoders[0].angle_now;
      if (abs(delta_angle) > (0.5f * angle_max_per_revolution)) {
        encoders[0].circle_num += (delta_angle > 0) ? -1 : 1;
      }

      /* 低通滤波获取总路程 */
      int angle_total_raw =(int)(((b * encoders[0].angle_total_now) + ((1 - b) *((encoders[0].circle_num * angle_max_per_revolution) +angle_raw))));
      float speed_raw =  (((float)(angle_total_raw - encoders[0].angle_total_now) / angle_max_per_revolution) / (encode_period_time) * 60);
      float delte_speed_raw = (fabsf(speed_raw - encoders[0].speed));
      if (((delte_speed_raw >= (120))) ) {
        speed_raw = encoders[0].speed;
      }
      speed_raw = ((c * encoders[0].speed) + ((1 - c) * speed_raw));
      encoders[0].angle_total_now = angle_total_raw;
      encoders[0].speed = speed_raw;
      encoders[0].angle_now = angle_raw;
      begin_flag = 0;
    }
    else {
      usart_printf("error\n");
      while(1);
    }
  }
}

// 获取速度 单位为圈/秒
float encode_get_speed(void) {
  return encoders[0].speed;
}

// 获取位置
int encode_get_location(void) { 
    return encoders[0].angle_total_now; 
}

/*************************************/
